﻿using System;
using System.Collections.Generic;
using Sandbox.Common.ObjectBuilders;
using Sandbox.Game;
using Sandbox.Game.Entities;
using Sandbox.Game.Entities.Character.Components;
using Sandbox.Game.EntityComponents;
using Sandbox.Game.Lights;
using Sandbox.ModAPI;
using SpaceEngineers.Game.ModAPI;
using VRage.Game;
using VRage.Game.Components;
using VRage.Game.ModAPI;
using VRage.ModAPI;
using VRage.ObjectBuilders;
using VRageMath;

namespace Vicizlat.MultifloorCargoElevator
{
    [MyEntityComponentDescriptor(typeof(MyObjectBuilder_ExtendedPistonBase), false, "VCZ_Cargo_Elevator_Piston")]
    public partial class MultifloorCargoElevator : MyGameLogicComponent
    {
        private readonly int MaxHeight = 200;   // Max Blocks the elevator can travel.
        private readonly float CABIN_SPHERE_RADIUS = 5.625f;
        private BoundingBoxD cabinBB = new BoundingBoxD(new Vector3D(-5.625f, 0.75f, -5.625f), new Vector3D(5.625f, 13.5f, 5.625f));    // LEFT, BOTTOM(-1.25f + 2.25), FRONT, RIGHT, TOP(11.25f + 2.25), BACK
        internal Utilities Utils = new Utilities();
        private bool DedicatedCheck;

        public IMyExtendedPistonBase Elevator_block;
        private IMyAdvancedDoor CurrentBlock;
        private IMyAdvancedDoor TargetBlock;
        private IMyAdvancedDoor NextShaftDoor;
        private IMyAdvancedDoor PrevShaftDoor;

        public bool CurrentFloorChanged = true;
        public int CurrentFloor = 0;
        private int oldCurrentFloor = 1;
        public bool TargetFloorChanged = true;
        public int TargetFloor = 0;
        private int oldTargetFloor = 1;
        public bool FloorsChanged = true;
        private IMyAdvancedDoor[] Floor = new IMyAdvancedDoor[9];
        private IMyAdvancedDoor[] oldFloor = new IMyAdvancedDoor[9];
        private float[] Distance = new float[9];

        public bool ShouldMoveEntity = false;
        public bool ElevatorStarted = true;
        private bool AllEntitiesContained;

        private string ElevatorIDs = "VCZ_Cargo_Elevator_Floor\nVCZ_Cargo_Elevator_Floor_Half\nVCZ_Cargo_Elevator_ShaftDoor\nVCZ_Cargo_Elevator_Filler1\nVCZ_Cargo_Elevator_Filler2\nVCZ_Cargo_Elevator_Filler3\nVCZ_Cargo_Elevator_Filler4";
        private List<IMyCubeBlock> ShaftDoorsList = new List<IMyCubeBlock>();
        private List<IMyEntity> entityList = new List<IMyEntity>();
        private List<Vector3D> offsetList = new List<Vector3D>();
        private List<MatrixD> orientList = new List<MatrixD>();
        private Vector3 SpeedInTransport = new Vector3(0);

        private bool ElevatorReady;
        private int FloorsCount;
        private int ElevatorHeight;

        public override void Init(MyObjectBuilder_EntityBase objectBuilder)
        {
            NeedsUpdate |= MyEntityUpdateEnum.BEFORE_NEXT_FRAME;
            Elevator_block = (IMyExtendedPistonBase)Entity;
        }

        public override void Close()
        {
            try
            {
                if (!MyAPIGateway.Utilities.IsDedicated)
                {
                    for (int i = 0; i <= 3; i++)
                    {
                        if (Light[i] != null)
                        {
                            MyLights.RemoveLight(Light[i]);
                            Light[i] = null;
                        }
                    }
                    if (SoundEmitter != null)
                    {
                        SoundEmitter.StopSound(true);
                        SoundEmitter = null;
                    }
                }
                if (ElevatorCabin != null) ElevatorCabin.CubeGrid.PositionComp.OnPositionChanged -= OnPositionChanged;
                Elevator_block.AppendingCustomInfo -= AppendingCustomInfo;
                Elevator_block.CubeGrid.OnBlockAdded -= OnBlockAddedOrRemoved;
                Elevator_block.CubeGrid.OnBlockRemoved -= OnBlockAddedOrRemoved;
            }
            catch (Exception e)
            {
                Logging.Instance.WriteLine("[ Error in 'Close': " + e.Message + " ]");
            }
            NeedsUpdate = MyEntityUpdateEnum.NONE;
        }

        public override void UpdateOnceBeforeFrame()
        {
            try
            {
                NeedsUpdate = MyEntityUpdateEnum.EACH_FRAME;

                DedicatedCheck = MyAPIGateway.Utilities.IsDedicated;
                LoadTerminalControlsSettings();
                if (!DedicatedCheck)
                {
                    CreateTerminalControls();
                    InitExtras();
                }
                CreateActions();
                Elevator_block.AppendingCustomInfo += AppendingCustomInfo;
                Elevator_block.CubeGrid.OnBlockAdded += OnBlockAddedOrRemoved;
                Elevator_block.CubeGrid.OnBlockRemoved += OnBlockAddedOrRemoved;
                FindFloors();
            }
            catch (Exception e)
            {
                Logging.Instance.WriteLine("[ Error: " + e.StackTrace + " ]");
            }
        }

        private void OnBlockAddedOrRemoved(IMySlimBlock Block)
        {
            if (ElevatorIDs.Contains(Block.BlockDefinition.Id.SubtypeName)) FindFloors();
        }

        public override void UpdateAfterSimulation()
        {
            try
            {
                if (Utils.DebugAll) Utils.Stopwatch.Restart();
                if (!CargoElevatorSession._init) return;
                if (Elevator_block.CubeGrid.Physics == null || !Elevator_block.IsFunctional) return;
                Utils.Timing();

                if (Utils.frame == 0) CheckAndGetTopPart();
                if (Utils.frame == 20) CheckTerminalControlSettings();
                if (Utils.frame == 40) SaveTerminalControlSettings();

                if (Elevator_block.ResourceSink.SuppliedRatioByType(MyResourceDistributorComponent.ElectricityId) != 1) return;

                if (ElevatorReady && CabinReady)
                {
                    GetCurrentFloor();
                    CloseAndLockFloors(false);
                    if (!DedicatedCheck)
                    {
                        CheckFloorDisplays();
                        UpdateFloorDisplays();
                        UpdateExtras();
                    }
                    if (ShouldMoveEntity)
                    {
                        if (TargetFloor > 0 && Elevator_block.CurrentPosition != Distance[TargetFloor - 1]) StartElevator();
                        if (ElevatorStarted) HoldElevator();
                        MoveEntity();
                    }
                    else UpdateEntities();
                    if (ElevatorStarted && TargetFloor > 0 && CurrentFloor == TargetFloor && Elevator_block.CurrentPosition == Distance[TargetFloor - 1]) SendStopSignal();
                }
                else CloseAndLockFloors(true);
                if (Utils.DebugAll) Utils.StopWatchReport("<| Whole Script Running |>");
            }
            catch (Exception e)
            {
                Logging.Instance.WriteLine("[ Error: " + e.StackTrace + " ]");
            }
        }

        private void OnPositionChanged(MyPositionComponentBase comp) => positionChanged = true;

        private void FindFloors()
        {
            FloorsCount = 0;
            for (int diff = 1; diff <= MaxHeight; diff++)
            {
                IMySlimBlock NextBlock = Elevator_block.CubeGrid.GetCubeBlock(Elevator_block.Position + (Vector3I)(Elevator_block.LocalMatrix.Up * diff));
                if (NextBlock == null || !ElevatorIDs.Contains(NextBlock.BlockDefinition.Id.SubtypeName)) break;
                if (NextBlock.BlockDefinition.Id.SubtypeName == "VCZ_Cargo_Elevator_ShaftDoor")
                {
                    ShaftDoorsList.Add(NextBlock.FatBlock);
                    continue;
                }
                if (NextBlock.BlockDefinition.Id.SubtypeName.Contains("VCZ_Cargo_Elevator_Floor"))
                {
                    if (FloorsCount > 0 && Floor[FloorsCount - 1] != null && NextBlock.FatBlock == Floor[FloorsCount - 1]) continue;
                    FloorsCount++;
                    Floor[FloorsCount - 1] = NextBlock.FatBlock as IMyAdvancedDoor;
                    Floor[FloorsCount - 1].CustomData = "Parent Elevator ID: [" + Elevator_block.EntityId + "]\nFloor No.: [" + FloorsCount + "]";
                    if (NextBlock.FatBlock.BlockDefinition.SubtypeId == "VCZ_Cargo_Elevator_Floor") Distance[FloorsCount - 1] = (diff - 1) * 2.5f;
                    else Distance[FloorsCount - 1] = ((diff - 1) * 2.5f) + 1.25f;
                    ElevatorHeight = diff;
                    if (FloorsCount == 9) break;
                }
            }
            ElevatorReady = FloorsCount >= 2 && FloorsCount <= 9;
            FloorsChanged = true;
            CurrentFloorChanged = true;
            Elevator_block.RefreshCustomInfo();
        }

        private void GetCurrentFloor()
        {
            CabinPosition = ElevatorCabin.WorldMatrix.Translation + (ElevatorCabin.WorldMatrix.Up * 2.5f);
            if (CurrentBlock == null || CurrentFloor == 0 || Elevator_block.Enabled)
            {
                IMySlimBlock Block = Elevator_block.CubeGrid.GetCubeBlock(Elevator_block.CubeGrid.WorldToGridInteger(CabinPosition));
                if (Block != null && ElevatorIDs.Contains(Block.BlockDefinition.Id.SubtypeName))
                {
                    if (Block.FatBlock is IMyAdvancedDoor && Block.BlockDefinition.Id.SubtypeName != "VCZ_Cargo_Elevator_ShaftDoor")
                    {
                        CurrentBlock = Block.FatBlock as IMyAdvancedDoor;
                        for (int i = 1; i <= FloorsCount; i++)
                        {
                            if (CurrentBlock == Floor[i - 1])
                            {
                                CurrentFloor = i;
                                break;
                            }
                        }
                        if (TargetFloor == 0 && TargetBlock == null)
                        {
                            TargetFloor = CurrentFloor;
                            TargetBlock = CurrentBlock;
                            CurrentFloorChanged = true;
                            FloorsChanged = true;
                        }
                    }
                    else if (CurrentFloor == 0)
                    {
                        TargetFloor = 1;
                        TargetBlock = Floor[0];
                        Elevator_block.MaxLimit = Distance[0];
                        Elevator_block.MinLimit = Distance[0];
                        if (Distance[0] > Elevator_block.CurrentPosition) Elevator_block.Extend();
                        else Elevator_block.Retract();
                        Elevator_block.Enabled = true;
                        ElevatorStarted = true;
                    }
                }
            }
        }

        private void CloseAndLockFloors(bool LockAllFloors)
        {
            if (FloorsCount > 0)
            {
                for (int i = 1; i <= FloorsCount; i++)
                {
                    if (!LockAllFloors && Floor[i - 1] == CurrentBlock) CurrentBlock.Enabled = !ElevatorStarted && CurrentBlock.OpenRatio == 2.4f ? false : true;
                    else
                    {
                        Floor[i - 1].Enabled = Floor[i - 1].OpenRatio == 0 ? false : true;
                        Floor[i - 1].CloseDoor();
                    }
                }
            }
            if (ShaftDoorsList.Count > 0)
            {
                foreach (IMyAdvancedDoor ShaftDoor in ShaftDoorsList)
                {
                    if (ShaftDoor == NextShaftDoor || ShaftDoor == PrevShaftDoor) continue;
                    else ShaftDoor.CloseDoor();
                }
            }
        }

        private void UpdateEntities()
        {
            MatrixD cabinMatrix = ElevatorCabin.WorldMatrix;
            if (Utils.Debug)
            {
                Color debugColor = new Color(50, 200, 128, 50);
                MySimpleObjectDraw.DrawTransparentBox(ref cabinMatrix, ref cabinBB, ref debugColor, MySimpleObjectRasterizer.SolidAndWireframe, 1, 0.01f);
            }
            var cabinOBB = new MyOrientedBoundingBoxD(cabinBB, cabinMatrix);
            var sphere = new BoundingSphereD(CabinPosition, CABIN_SPHERE_RADIUS);
            AllEntitiesContained = true;
            entityList.Clear();
            offsetList.Clear();
            orientList.Clear();
            foreach (IMyEntity entity in MyAPIGateway.Entities.GetTopMostEntitiesInSphere(ref sphere))
            {
                if (entity is IMyCharacter || entity is IMyFloatingObject || entity is IMyInventoryBag || entity is IMyCubeGrid)
                {
                    if (entity is IMyCubeGrid)
                    {
                        if (entity as IMyCubeGrid == Elevator_block.CubeGrid) continue;
                        if (entity as IMyCubeGrid == ElevatorCabin.CubeGrid) continue;
                        if ((entity as IMyCubeGrid).Physics == null) continue;
                    }
                    var minEntity = entity.Model != null ? entity.Model.BoundingBox.Min : entity.LocalAABB.Min;
                    var maxEntity = entity.Model != null ? entity.Model.BoundingBox.Max : entity.LocalAABB.Max;
                    var entityOBB = new MyOrientedBoundingBoxD(new BoundingBoxD(minEntity, maxEntity), entity.WorldMatrix);
                    if (cabinOBB.Contains(ref entityOBB) == ContainmentType.Contains)
                    {
                        if (entity is IMyCubeGrid && entityList.Count > 0)
                        {
                            bool skip = false;
                            for (int i = 0; i < entityList.Count; i++)
                            {
                                if (entityList[i] is IMyCubeGrid && (entityList[i] as MyCubeGrid).BlocksCount > 0)
                                {
                                    if ((entity as MyCubeGrid).IsSameConstructAs((entityList[i] as IMyCubeGrid)))
                                    {
                                        skip = true;
                                        if ((entityList[i] as MyCubeGrid).BlocksCount > (entity as MyCubeGrid).BlocksCount) continue;
                                        else
                                        {
                                            entityList[i] = entity;
                                            offsetList[i] = entity.WorldMatrix.Translation - CabinPosition;
                                            orientList[i] = entity.WorldMatrix;
                                            break;
                                        }
                                    }
                                }
                            }
                            if (skip) continue;
                        }
                        entityList.Add(entity);
                        offsetList.Add(entity.WorldMatrix.Translation - CabinPosition);
                        orientList.Add(entity.WorldMatrix);
                    }
                    if (cabinOBB.Contains(ref entityOBB) == ContainmentType.Intersects) AllEntitiesContained = false;
                }
            }
        }

        public void SendToFloor(int i) => Communication.RequestFloor(0, i, Elevator_block.EntityId);

        public void SendStopSignal() => Communication.RequestStop(1, Elevator_block.EntityId);

        public void SetTargetFloor(int selectedFloor)
        {
            if (selectedFloor <= FloorsCount && AllEntitiesContained)
            {
                if (selectedFloor == CurrentFloor && Elevator_block.CurrentPosition != Distance[selectedFloor - 1])
                {
                    if (Elevator_block.CurrentPosition < Distance[selectedFloor - 1]) CurrentFloor = selectedFloor - 1;
                    else CurrentFloor = selectedFloor + 1;
                    CurrentFloorChanged = true;
                }
                if (selectedFloor != CurrentFloor)
                {
                    Logging.Instance.WriteLine("Setting Target Floor: " + selectedFloor);
                    TargetBlock = Floor[selectedFloor - 1];
                    TargetFloor = selectedFloor;
                    ShouldMoveEntity = true;
                }
            }
        }

        private void StartElevator()
        {
            Elevator_block.MaxLimit = Distance[TargetFloor - 1];
            Elevator_block.MinLimit = Distance[TargetFloor - 1];
            if (Distance[TargetFloor - 1] > Elevator_block.CurrentPosition) Elevator_block.Extend();
            else Elevator_block.Retract();
            if (CurrentBlock != null && CurrentFloor <= FloorsCount)
            {
                CurrentBlock.Enabled = true;
                CurrentBlock.CloseDoor();
                if (CurrentBlock.OpenRatio == 0 && TargetFloor <= FloorsCount && !ElevatorStarted)
                {
                    Elevator_block.Enabled = true;
                    ElevatorStarted = true;
                }
            }
            else
            {
                Elevator_block.Enabled = true;
                ElevatorStarted = true;
            }
        }

        private void HoldElevator()
        {
            NextShaftDoor = null;
            PrevShaftDoor = null;
            int detectionDistance = Math.Abs((int)Elevator_block.Velocity);
            if (Elevator_block.Velocity > 0)
            {
                NextShaftDoor = GetBlockAbove(detectionDistance + 4);
                PrevShaftDoor = GetBlockBelow(1);
                if (NextShaftDoor != null)
                {
                    NextShaftDoor.OpenDoor();
                    if (Vector3D.Distance(NextShaftDoor.WorldMatrix.Translation, CabinPosition) <= 12.5f && NextShaftDoor.OpenRatio < 1.25f)
                    {
                        Elevator_block.Enabled = false;
                    }
                    else Elevator_block.Enabled = true;
                }
                else Elevator_block.Enabled = true;
            }
            else
            {
                NextShaftDoor = GetBlockBelow(detectionDistance + 4);
                PrevShaftDoor = GetBlockAbove(5);
                if (NextShaftDoor != null)
                {
                    NextShaftDoor.OpenDoor();
                    if (Vector3D.Distance(NextShaftDoor.WorldMatrix.Translation, CabinPosition) <= 3.5f && NextShaftDoor.OpenRatio < 1.25f)
                    {
                        Elevator_block.Enabled = false;
                    }
                    else Elevator_block.Enabled = true;
                }
                else Elevator_block.Enabled = true;
            }
            if (PrevShaftDoor != null) PrevShaftDoor.OpenDoor();
        }

        private IMyAdvancedDoor GetBlockBelow(int detectionDistance)
        {
            IMyAdvancedDoor Block = null;
            for (int diff = 0; diff <= detectionDistance; diff++)
            {
                Vector3I PrevBlockPos = Elevator_block.CubeGrid.WorldToGridInteger(CabinPosition) + (Vector3I)(Elevator_block.LocalMatrix.Down * diff);
                IMySlimBlock PrevBlock = Elevator_block.CubeGrid.GetCubeBlock(PrevBlockPos);
                if (PrevBlock != null && PrevBlock.BlockDefinition.Id.SubtypeName == "VCZ_Cargo_Elevator_ShaftDoor")
                {
                    if (Vector3D.Distance(PrevBlock.FatBlock.WorldMatrix.Translation, CabinPosition) < Math.Abs(Distance[TargetFloor - 1] - Elevator_block.CurrentPosition))
                    {
                        Block = PrevBlock.FatBlock as IMyAdvancedDoor;
                        if (Block.OpenRatio == 1.25f)
                        {
                            Block.OpenDoor();
                            continue;
                        }
                        break;
                    }
                }
            }
            return Block;
        }

        private IMyAdvancedDoor GetBlockAbove(int detectionDistance)
        {
            IMyAdvancedDoor Block = null;
            for (int diff = 0; diff <= detectionDistance; diff++)
            {
                Vector3I NextBlockPos = Elevator_block.CubeGrid.WorldToGridInteger(CabinPosition) + (Vector3I)(Elevator_block.LocalMatrix.Up * diff);
                IMySlimBlock NextBlock = Elevator_block.CubeGrid.GetCubeBlock(NextBlockPos);
                if (NextBlock != null && NextBlock.BlockDefinition.Id.SubtypeName == "VCZ_Cargo_Elevator_ShaftDoor")
                {
                    if (Vector3D.Distance(NextBlock.FatBlock.WorldMatrix.Translation, CabinPosition) < Math.Abs(Distance[TargetFloor - 1] - Elevator_block.CurrentPosition))
                    {
                        Block = NextBlock.FatBlock as IMyAdvancedDoor;
                        if (Block.OpenRatio == 1.25f)
                        {
                            Block.OpenDoor();
                            continue;
                        }
                        break;
                    }
                }
            }
            return Block;
        }

        private void MoveEntity()
        {
            for (int i = 0; i < entityList.Count; i++)
            {
                if (entityList[i] != null)
                {
                    var newPosition = CabinPosition + offsetList[i] + (ElevatorCabin.WorldMatrix.Up * 0.11f);
                    if (entityList[i] is IMyCharacter && (entityList[i] as IMyCharacter).EquippedTool == null)
                    {
                        if (entityList[i].Components.Get<MyCharacterDetectorComponent>()?.UseObject != null)
                        {
                            var dummy = entityList[i].Components.Get<MyCharacterDetectorComponent>()?.UseObject.Dummy as IMyModelDummy;
                            bool UseDummy = MyAPIGateway.Input.IsGameControlReleased(MyControlsSpace.USE) || MyAPIGateway.Input.IsGameControlReleased(MyControlsSpace.PRIMARY_TOOL_ACTION);
                            if (dummy.Name.Contains("detector_ladder") && UseDummy)
                            {
                                entityList[i] = null;
                                continue;
                            }
                        }
                    }
                    if (entityList[i] is IMyCubeGrid)
                    {
                        //(entityList[i] as MyCubeGrid).PositionComp.SetWorldMatrix(orientList[i]);
                        var wm = orientList[i];
                        (entityList[i] as MyCubeGrid).PositionComp.SetWorldMatrix(ref wm);

                        foreach (var block in (entityList[i] as MyCubeGrid).GetFatBlocks())
                        {
                            if (block is IMyLandingGear && (block as IMyLandingGear).IsLocked && (block as IMyLandingGear).GetAttachedEntity() == ElevatorCabin.CubeGrid) (block as IMyLandingGear).Unlock();
                        }
                    }
                    if (entityList[i] is IMyCharacter) entityList[i].PositionComp.SetPosition(newPosition);
                    else entityList[i].PositionComp.SetPosition(CabinPosition + offsetList[i]);
                    entityList[i].Physics.SetSpeeds(SpeedInTransport, SpeedInTransport);
                }
            }
        }

        public void StopElevator()
        {
            if (ShouldMoveEntity)
            {
                Logging.Instance.WriteLine("Target Floor Reached: Stopping Elevator");
                if (ShouldPlayDingSound) PlayDingSound = true;
                ShouldMoveEntity = false;
            }
            ElevatorStarted = false;
            Elevator_block.Enabled = false;
            if (Distance[TargetFloor - 1] == Elevator_block.CurrentPosition)
            {
                TargetBlock.Enabled = true;
                TargetBlock.OpenDoor();
            }
            CurrentFloorChanged = true;
            FloorsChanged = true;
            if (PrevShaftDoor != null) PrevShaftDoor.CloseDoor();
            if (NextShaftDoor != null) NextShaftDoor.CloseDoor();
        }
    }
}